#include "spokeDart.h"
#include "randUtil.h"
#include "geometry.h"
#include <algorithm>
#include "dataType.h"

using namespace std;

SpokeDart::SpokeDart()
{
}

SpokeDart::~SpokeDart()
{
}

void SpokeDart::reset(unsigned int d, const VectorD& front, DATA_TYPE radius)
{
	dimensions_ = d;
	invDimensions_ = (DATA_TYPE)1 / dimensions_;
	refPoint_ = front;
	direction_ = VectorD::gaussianRandomVector();
	direction_.normalize();

	DATA_TYPE intervalMin = -radius * 2;
	DATA_TYPE intervalMax = radius * 2;
	for (unsigned int i = 0; i < d; ++i)
	{
		if (direction_[i] > 0)
		{
			DATA_TYPE t = ((DATA_TYPE)1 - refPoint_[i]) / direction_[i];
			intervalMax = min(intervalMax, t);

			t = (0 - refPoint_[i]) / direction_[i];
			intervalMin = max(intervalMin, t);
		}
		else if (direction_[i] < 0)
		{
			DATA_TYPE t = (0 - refPoint_[i]) / direction_[i];
			intervalMax = min(intervalMax, t);

			t = (1 - refPoint_[i]) / direction_[i];
			intervalMin = max(intervalMin, t);
		}
	}

	intervals_.clear();
	if (intervalMin < -radius)
		intervals_.push_back(Interval(intervalMin, -radius));
	if (intervalMax > radius)
		intervals_.push_back(Interval(radius, intervalMax));
}

void SpokeDart::updateIntervals(const vector<VectorD>& points, DATA_TYPE radius)
{
	for (unsigned int i = 0; i < points.size(); ++i)
	{
		if (intervals_.size() == 0)
			return;

		const VectorD& point = points[i];

		DATA_TYPE x0, x1;
		if (raySphereIntersect(point, radius, refPoint_, direction_, x0, x1) == false)
			continue;

		Interval newInterval(x0, x1);

		list<Interval>::iterator low, up, tempIt;
		low = lower_bound(intervals_.begin(), intervals_.end(), newInterval, Dart::intervalBeginCompare);
		tempIt = low;
		if (tempIt != intervals_.begin())
			--tempIt;
		up = upper_bound(tempIt, intervals_.end(), newInterval, Dart::intervalEndCompare);
		DATA_TYPE min = intervals_.front().begin_;
		DATA_TYPE max = intervals_.back().end_;

		tempIt = up;
		// split an interval
		if (up != intervals_.end() && ++tempIt == low)
		{
			DATA_TYPE splitEnd = up->end_;
			up->end_ = x0;
			intervals_.insert(low, Interval(x1, splitEnd));
		}
		else
		{
			if (x0 > min)
			{
				tempIt = low;
				--tempIt;
				if (x0 < tempIt->end_)
					tempIt->end_ = x0;
			}
			if (x1 < max)
			{
				if (x1 > up->begin_)
					up->begin_ = x1;
			}
			intervals_.erase(low, up);
		}
	}
}

bool SpokeDart::pickPoint(VectorD& point)
{
	int numIntervals = intervals_.size();
	if (numIntervals == 0)
		return false;

	// raise power
	for (list<Interval>::iterator it = intervals_.begin(); it != intervals_.end(); ++it)
	{
		it->begin_ = raisePower(it->begin_);
		it->end_ = raisePower(it->end_);
	}

	DATA_TYPE totalLength = 0;
	for (list<Interval>::const_iterator it = intervals_.begin(); it != intervals_.end(); ++it)
	{
		totalLength += it->getLength();
	}

	DATA_TYPE randPoint = RandUtil::randRange(totalLength);
	for (list<Interval>::const_iterator it = intervals_.begin(); it != intervals_.end(); ++it)
	{
		DATA_TYPE length = it->getLength();
		if (randPoint <= length)
		{
			DATA_TYPE t = it->begin_ + randPoint;
			point = refPoint_ + lowerPower(t) * direction_;
			return true;
		}
		randPoint -= length;
	} 
	point = refPoint_ + lowerPower(intervals_.back().end_) * direction_;

	return false;
}